#include "lib.h"

void pidReset(P_PID_HANDLE phdl)
{
    phdl->sum_error     = 0;
    phdl->last_error    = 0;
    filter1Reset(&phdl->hfilter1);
}

void pidEnable(P_PID_HANDLE phdl)
{
    phdl->enable_lim_sum_error  = 1;    //使能积分限幅
    phdl->enable_lim_ouput      = 1;    //使能输出限幅
    phdl->enable                = 1;    //使能输出
    phdl->hfilter1.a            = 0;    //使滤波参数为0
}

double pidProcess(P_PID_HANDLE phdl, double input, double measure)
{
	//计算误差和累计误差
	phdl->error = input - measure;
    phdl->sum_error += phdl->error;
    //误差积分限幅
    if(phdl->enable_lim_sum_error == 1 && fabs(phdl->sum_error) > phdl->lim_sum_error)
    {
        if(phdl->sum_error > 0)
            phdl->sum_error = phdl->lim_sum_error;
        else
            phdl->sum_error = -phdl->lim_sum_error;
    }
	//PID输出
    phdl->pid_output =        
					   phdl->kp * phdl->error
					 + phdl->ki * phdl->sum_error
					 + phdl->kd * (phdl->error - phdl->last_error);
	//记录kp、ki、kd输出
    phdl->kp_output = phdl->kp * phdl->error;
    phdl->ki_output = phdl->ki * phdl->sum_error;
    phdl->kd_output = phdl->kd * (phdl->error - phdl->last_error) ;
    
    phdl->error_dec = phdl->error - phdl->last_error;
		
    //输出限幅
    if(phdl->enable_lim_ouput == 1 && fabs(phdl->pid_output) > phdl->lim_output)
    {
        if( phdl->pid_output > 0)
             phdl->pid_output = phdl->lim_output;
        else
             phdl->pid_output = -phdl->lim_output;
    }

    //更新误差值
    phdl->last_error = phdl->error;

    //是否使能输出
    if(phdl->enable == 1)
        return  phdl->pid_output;
    else
        return 0;
}

